#include "VideoGrabber.h"
#include <aruco\aruco.h>

using namespace aruco;

VideoGrabber::VideoGrabber(void)
{
	vcap.open(CV_CAP_OPENNI);
	vwri.open("grabbed.avi",-1,25.0,Size(640,480));

}

void VideoGrabber::start(){
	Mat rgb;
	MarkerDetector detector;
	vcap.grab();
	while(vcap.isOpened()){
		vcap.retrieve(rgb,CV_CAP_OPENNI_BGR_IMAGE);
		CameraParameters CamParam;
		CamParam.readFromXMLFile("camera.yml");

		//resizes the parameters to fit the size of the input image
		CamParam.resize(rgb.size());

		//Ok, let's detect
		detector.detect(rgb, detectedMarkers, CamParam, 205.0 / 1000.0);


		if(detectedMarkers.empty()){
			vwri << rgb;
			Sleep(40);
			continue;
		}

		//draw a 3d cube in each marker if there is 3d info
		if (CamParam.isValid()) {
			for (unsigned int i = 0; i < detectedMarkers.size(); i++){
				CvDrawingUtils::draw3dCube(rgb, detectedMarkers[i], CamParam);
				detectedMarkers[i].draw(rgb, Scalar(0, 0, 255), 2);

			}
			vwri << rgb;
		}
		Sleep(40);
	}
}

void VideoGrabber::stop(){
	vcap.release();
	Sleep(50);
	vwri.release();

}

VideoGrabber::~VideoGrabber(void)
{
}


